package com.hitqz.robot.dispatch.netty.service.handle.impl;

import com.alibaba.fastjson.JSONObject;
import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.google.common.collect.Maps;
import com.google.common.util.concurrent.RateLimiter;
import com.hitqz.robot.api.business.entity.JobModeEntity;
import com.hitqz.robot.api.business.entity.PathResourceEntity;
import com.hitqz.robot.api.business.enums.WorkStatusEnum;
import com.hitqz.robot.api.business.model.dto.FlowPatrolPreAndEndTicketsDto;
import com.hitqz.robot.api.common.dto.info.RobotNavInfoDto;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotApplyGotoTargetDto;
import com.hitqz.robot.api.dispatch.protocol.function.CheckOperationFunction;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Component;
import org.springframework.util.StringUtils;

import java.util.*;
import java.util.concurrent.*;
import java.util.stream.Collectors;


/**
 * @author kehanjiang
 * @date 2023/11/03
 */
@Slf4j
@Component
public class LunduiRuleHandle extends AbstractRuleHandle {
    private final static RateLimiter logLimiter = RateLimiter.create(1);

    /**
     * 这些起点 前往南线或者北线需要中转
     */
    private List<String> specialPos = Arrays.asList("CP14", "CP15", "LM11", "LM9", "LM7", "LM8");

    @Override
    public String getType() {
        return "lundui";
    }

    /**
     * 生成 前往任务
     *
     * @param dto
     */
    @Override
    public void applyGotoTarget(RobotApplyGotoTargetDto dto) {
        String clientId = dto.getClientId();
        String currentPos = dto.getCurrentPos();
        String targetPos = dto.getTargetPos();
        LinkedHashMap<String, CheckOperationFunction> middlePos = Optional.ofNullable(dto.getMiddlePos())
                .orElse(new LinkedHashMap<>());
        JobModeEntity jobMode = jobModeService.getDefaultJobMode();
        if ("1".equals(jobMode.getMode())) {//单车模式
            cmdRobot2Pos(clientId, targetPos);
        } else {
            //"CP14", "CP15", "LM11", "LM9", "LM7", "LM8"起点 前往南线或者北线需要中转，以保证走空车道
            if (specialPos.contains(currentPos) && "LM4".equals(targetPos)) {//前往南线
                //先前往 LM45 再前往 LM4
                middlePos.put("LM45", null);
            } else if (specialPos.contains(currentPos) && "LM2".equals(targetPos)) {//前往北线
                //先前往 LM46 再前往 LM2
                middlePos.put("LM46", null);
            }
            this.submitTask(clientId, currentPos, middlePos, targetPos);
        }
    }

    /**
     * 提交前往任务
     *
     * @param clientId
     * @param currentPos   当前点位
     * @param middlePosMap 中转点位
     * @param targetPos    目标点位
     */
    private void submitTask(String clientId, String currentPos, LinkedHashMap<String, CheckOperationFunction> middlePosMap, String targetPos) {
        FutureTask<String> futureTask = new FutureTask<>(() -> {
            try {
                LinkedHashMap<String, CheckOperationFunction> allPosMap = Maps.newLinkedHashMap(middlePosMap);
                allPosMap.put(targetPos, null);
                for (Map.Entry<String, CheckOperationFunction> posEntry : allPosMap.entrySet()) {
                    String target = posEntry.getKey();
                    String current = this.getCurrentAgvPos(clientId);
                    List<RobotMapPos> navPath = getNavPath(getRoundRobinUrl(), current, target);
                    log.warn("[{}]获取到的navPath{}", clientId, navPath.stream().map(RobotMapPos::getCode).toList());
                    //计算出所需占用的路径资源
                    List<PathResourceEntity> allPathResourceList = pathResourceMapper.selectList(null);
                    List<PathResourceEntity> needPathResourceList = new ArrayList<>();
                    for (RobotMapPos robotMapPos : navPath) {
                        String code = robotMapPos.getCode();
                        for (PathResourceEntity pathResourceEntity : allPathResourceList) {
                            List<String> posList = List.of(pathResourceEntity.getPathList().split(","));
                            if (posList.contains(code) && !needPathResourceList.contains(pathResourceEntity)) {
                                needPathResourceList.add(pathResourceEntity);
                            }
                        }
                    }
                    log.warn("[{}]获取到的needPathResourceList{}", clientId, needPathResourceList.stream().map(PathResourceEntity::getId).toList());
                    Long lastResourceId = null;
                    //资源申请，申请到就走，申请不到就等待
                    for (PathResourceEntity pathResourceEntity : needPathResourceList) {
                        log.warn("[{}]==>即将申请的资源{}", clientId, pathResourceEntity.getPathList());
                        Long resourceId = pathResourceEntity.getId();
                        //是否需要重新申请路径
                        boolean changePath = this.changePath(clientId, resourceId);
                        if (changePath) break;
                        //申请资源,申请不到就等待
                        this.applyResource(clientId, resourceId);
                        //申请成功，走到资源允许范围内 离目标点位最近的点，同时标记上一个资源已完成占用 状态改为2
                        this.goTargetInResourceRange(clientId, navPath, resourceId, lastResourceId);
                        lastResourceId = resourceId;
                    }
                    // 轮对前往中间点位是为了能走空车道，到了中间点位后可以直接继续前往目标点位
                    // 轮轴前往中间点位是为了避让其他AGV，到了中间点位后需要满足一定条件才能继续前往下个点位
                    CheckOperationFunction checkOperationFunction = posEntry.getValue();
                    if (checkOperationFunction != null) {
                        checkOperationFunction.waitUtilRelease();
                    }
                }
            } catch (Exception e) {
                log.error(e.getMessage(), e);
            }
            return "";
        });
        threadPoolExecutor.execute(futureTask);
        List<FutureTask> futureTaskList = futureTaskMap.computeIfAbsent(clientId, k -> new ArrayList<>());
        futureTaskList.add(futureTask);
    }

    /**
     * 是否需要重新申请路径(可能需要前往现有路径的其他点位上避让一下高优先级任务)
     * <p>
     * 轮对不需避让
     *
     * @param clientId
     * @param resourceId
     * @return
     */
    private boolean changePath(String clientId, Long resourceId) {
        return false;
    }

    /**
     * 申请成功，走到资源允许范围内 离目标点位最近的点，同时标记上一个资源已完成占用 状态改为2
     *
     * @param clientId
     * @param navPath
     * @param resourceId
     * @param lastResourceId
     */
    private void goTargetInResourceRange(String clientId, List<RobotMapPos> navPath, Long resourceId, Long lastResourceId) throws Exception {
        List<String> navPathCode = navPath.stream().map(RobotMapPos::getCode).collect(Collectors.toList());
        PathResourceEntity pathResourceEntity = pathResourceMapper.selectById(resourceId);
        List<String> posList = List.of(pathResourceEntity.getPathList().split(","));
        Map<String, Integer> posDisMap = new HashMap<>();
        for (String pos : posList) {
            int index = navPathCode.indexOf(pos);
            posDisMap.put(pos, index);
        }
        // 找到资源内离目标点位最近的点
        Map.Entry<String, Integer> maxEntry = posDisMap.entrySet()
                .stream()
                .max(Map.Entry.comparingByValue()).orElse(null);
        String targetPos = maxEntry.getKey();
        log.info("[{}] 资源内：{} 离目标点位:{} 最近的点是:{} ", clientId, posList,
                navPathCode.get(navPathCode.size() - 1), targetPos);
        // 前往目标点位
        this.cmdRobot2Pos(clientId, targetPos);

        String current = this.getCurrentAgvPos(clientId);
        //标记上个资源已占用结束
        if (lastResourceId != null) {
            PathResourceEntity lastPathResourceEntity = pathResourceMapper.selectById(lastResourceId);
            List<String> lastPosCode = List.of(lastPathResourceEntity.getPathList().split(","));
            while (lastPosCode.contains(current) || !StringUtils.hasText(current)) {
                current = this.getCurrentAgvPos(clientId);
                TimeUnit.SECONDS.sleep(1);
            }
            log.info("[{}] 当前位置：{}，已离开资源{}，开始标记资源占用结束！", clientId, current, lastPosCode);
            int updateCnt = pathResourceMapper.update(Wrappers.<PathResourceEntity>lambdaUpdate()
                    .set(PathResourceEntity::getStatus, "2")
                    .eq(PathResourceEntity::getRobotSn, clientId)
                    .eq(PathResourceEntity::getId, lastResourceId)
            );
            log.info("[{}] 更新{}条数据", clientId, updateCnt);
        }

        //等待AGV到达目标点位
        while (!targetPos.equals(current)) {
            current = this.getCurrentAgvPos(clientId);
            TimeUnit.SECONDS.sleep(1);
        }
        log.info("[{}] 达到位置:{} ", clientId, targetPos);
    }

    /**
     * 申请资源,申请不到就等待
     *
     * @param clientId
     * @param resourceId
     * @throws Exception
     */
    private void applyResource(String clientId, Long resourceId) throws Exception {
        //校验 阻塞 直到允许申请到资源
        this.checkOtherAgvTask(clientId, resourceId);
        this.allowApplyResource(clientId, resourceId);

        PathResourceEntity resource = pathResourceMapper.selectById(resourceId);
        while (!(
                ("0".equals(resource.getStatus()) && !StringUtils.hasText(resource.getRobotSn()))
                        || (!"0".equals(resource.getStatus()) && clientId.equals(resource.getRobotSn()))
        )) {
            resource = pathResourceMapper.selectById(resourceId);
            TimeUnit.SECONDS.sleep(1);
            if (logLimiter.tryAcquire(5)) {
                log.warn("[{}] 申请不到资源 {}", clientId, resource.getPathList());
            }
        }
        pathResourceMapper.update(Wrappers.<PathResourceEntity>lambdaUpdate()
                .set(PathResourceEntity::getStatus, "1")
                .set(PathResourceEntity::getRobotSn, clientId)
                .eq(PathResourceEntity::getId, resourceId)
        );
    }

    /**
     * 根据其他AGV任务及位置 判断是否允许申请资源
     * 南向北 北向南
     *
     * @param clientId
     * @param resourceId
     * @return
     */
    private void checkOtherAgvTask(String clientId, Long resourceId) throws Exception {
        RobotNavInfoDto currentAgvDto = this.getCurrentAgvDto(clientId);
        RobotNavInfoDto otherAgvDto = this.getOtherAgvDto(clientId);

        if (currentAgvDto == null || otherAgvDto == null) return;
        String currentPos = this.getCurrentAgvPos(clientId);
        String otherPos = this.getOtherAgvPos(clientId);

        String otherTaskParams = otherAgvDto.getTaskParams();
        if (!StringUtils.hasText(otherTaskParams)) return;
        FlowPatrolPreAndEndTicketsDto otherDto = JSONObject.parseObject(otherTaskParams, FlowPatrolPreAndEndTicketsDto.class);
        if (otherDto == null) return;
        String otherSourcePosCode = Optional.ofNullable(otherDto.getModalData().get("sourcePosName"))
                .orElse(otherDto.getModalData().get("sourcePosCode")).toString();
        String otherTargetPosCode = Optional.ofNullable(otherDto.getModalData().get("targetPosName"))
                .orElse(otherDto.getModalData().get("targetPosCode")).toString();

        log.info("[{}] otherSourcePosCode:{} otherTargetPosCode:{}", clientId, otherSourcePosCode, otherTargetPosCode);

        String currTaskParams = currentAgvDto.getTaskParams();
        if (!StringUtils.hasText(currTaskParams)) return;
        FlowPatrolPreAndEndTicketsDto currDto = JSONObject.parseObject(currTaskParams, FlowPatrolPreAndEndTicketsDto.class);
        if (currDto == null) return;
        String currSourcePosCode = Optional.ofNullable(currDto.getModalData().get("sourcePosName"))
                .orElse(currDto.getModalData().get("sourcePosCode")).toString();
        String currTargetPosCode = Optional.ofNullable(currDto.getModalData().get("targetPosName"))
                .orElse(currDto.getModalData().get("targetPosCode")).toString();
        log.info("[{}] currSourcePosCode:{} currTargetPosCode:{}", clientId, currSourcePosCode, currTargetPosCode);

        if (resourceId.intValue() == 142) {//LM5,LM38
            //其他AGV任务为 北向南时(911007 -> 911008)
            if ("911007".equals(otherSourcePosCode) && "911008".equals(otherTargetPosCode)) {
                if (currentPos.equals("LM45")) {
                    //当 LM46,LM4,LM40,LM5,LM38,LM51,LM41,LM3,LM39,LM2 没有机器人时才允许申请该资源
                    List<String> posList = List.of("LM46", "LM4", "LM40", "LM5", "LM38", "LM51", "LM41", "LM3", "LM39", "LM2");
                    String otherTargetPos = this.getOtherTargetPos(otherAgvDto.getTargetPos());
                    while (posList.contains(otherPos) && WorkStatusEnum.WORK.getCode().equals(otherAgvDto.getWorkStatus())) {
                        //当 机器人前往目标为 LM11时 空说明机器人已放完轮对，也可以申请资源
                        if ("LM11".equals(otherTargetPos)) {
                            break;
                        }
                        otherPos = this.getOtherAgvPos(clientId);
                        otherAgvDto = this.getOtherAgvDto(clientId);
                        otherTargetPos = this.getOtherTargetPos(otherAgvDto.getTargetPos());
                        TimeUnit.SECONDS.sleep(1);
                    }
                }
            }
        }
        if (resourceId.intValue() == 143) {//LM41,LM3,LM35
            //当前AGV任务为 北向南时(911007 -> 911008)
            if ("911007".equals(currSourcePosCode) && "911008".equals(currTargetPosCode)) {
                //其他AGV任务为 北向南时(911007 -> 911008)  或者 南面取轮对时(911008 -> ?)
                if (("911007".equals(otherSourcePosCode) && "911008".equals(otherTargetPosCode))
                        || "911008".equals(otherSourcePosCode)
                ) {
                    if (currentPos.equals("LM46")) {
                        //当 LM4,LM40,LM5,LM38,LM51,LM41,LM3,LM39,LM2 没有机器人时才允许申请该资源
                        List<String> posList = List.of("LM4", "LM40", "LM5", "LM38", "LM51", "LM41", "LM3", "LM39", "LM2");
                        while (posList.contains(otherPos) && WorkStatusEnum.WORK.getCode().equals(otherAgvDto.getWorkStatus())) {
                            otherPos = this.getOtherAgvPos(clientId);
                            otherAgvDto = this.getOtherAgvDto(clientId);
                            TimeUnit.SECONDS.sleep(1);
                        }
                    }
                }
            }
        }
    }

    private String getOtherTargetPos(RobotMapPos targetPos) {
        if (targetPos == null) return "";
        return targetPos.getCode();
    }

    /**
     * 判断是否允许申请资源 （就算资源空闲也要判断能否申请）
     *
     * @param clientId
     * @param resourceId
     * @return
     */
    private void allowApplyResource(String clientId, Long resourceId) throws Exception {
        RobotNavInfoDto currentAgvDto = this.getCurrentAgvDto(clientId);
        RobotNavInfoDto otherAgvDto = this.getOtherAgvDto(clientId);
        if (currentAgvDto == null || otherAgvDto == null) return;
        if (resourceId.intValue() == 116) {//LM9
            String currentPos = this.getCurrentAgvPos(clientId);
            String otherPos = this.getOtherAgvPos(clientId);
            String otherTargetPos = this.getOtherTargetPos(otherAgvDto.getTargetPos());
            if (List.of("LM7", "LM8").contains(currentPos)) {
                //其他AGV在LM11 且目标点位为LM7,LM8，则让其他AGV先申请LM9资源 否则会堵住
                while ("LM11".equals(otherPos) && List.of("LM7", "LM8").contains(otherTargetPos)) {
                    otherPos = this.getOtherAgvPos(clientId);
                    otherAgvDto = this.getOtherAgvDto(clientId);
                    otherTargetPos = this.getOtherTargetPos(otherAgvDto.getTargetPos());
                    TimeUnit.SECONDS.sleep(1);
                }
                //其他AGV在LM7或LM8 且其他AGV任务优先级高，则让其他AGV先申请LM9资源
                while (List.of("LM7", "LM8").contains(otherPos) && WorkStatusEnum.WORK.getCode().equals(otherAgvDto.getWorkStatus())
                        && Long.parseLong(currentAgvDto.getTaskNo()) > Long.parseLong(otherAgvDto.getTaskNo())) {
                    otherPos = this.getOtherAgvPos(clientId);
                    currentAgvDto = this.getCurrentAgvDto(clientId);
                    otherAgvDto = this.getOtherAgvDto(clientId);
                    TimeUnit.SECONDS.sleep(1);
                }
            }
        }
        if (resourceId.intValue() == 117) {//LM11
            String currentPos = this.getCurrentAgvPos(clientId);
            String otherPos = this.getOtherAgvPos(clientId);
            String currentTargetPos = currentAgvDto.getTargetPos().getCode();
            String otherTargetPos = this.getOtherTargetPos(otherAgvDto.getTargetPos());
            if (currentPos.equals("LM10")) {
                List<String> posList = List.of("LM36", "LM12", "LM1");
                //当其他AGV在LM36,LM12,LM1 且当前AGV目标点位是LM12，则让其他AGV先申请LM11资源 否则会堵住
                while (posList.contains(otherPos)
                        && "LM12".equals(currentTargetPos)
                        && WorkStatusEnum.WORK.getCode().equals(otherAgvDto.getWorkStatus())) {
                    otherPos = this.getOtherAgvPos(clientId);
                    otherAgvDto = this.getOtherAgvDto(clientId);
                    currentTargetPos = currentAgvDto.getTargetPos().getCode();
                    TimeUnit.SECONDS.sleep(1);
                }
                //当其他AGV在LM9 且当前AGV目标点位是LM7或LM8，则让其他AGV先申请LM11资源 否则会堵住
                while ("LM9".equals(otherPos)
                        && List.of("LM7", "LM8").contains(otherTargetPos)
                        && WorkStatusEnum.WORK.getCode().equals(otherAgvDto.getWorkStatus())) {
                    otherPos = this.getOtherAgvPos(clientId);
                    currentAgvDto = this.getCurrentAgvDto(clientId);
                    otherAgvDto = this.getOtherAgvDto(clientId);
                    otherTargetPos = this.getOtherTargetPos(otherAgvDto.getTargetPos());
                    TimeUnit.SECONDS.sleep(1);
                }
            } else if (currentPos.equals("LM36")) {
                List<String> posList = List.of("LM10", "LM9");
                //当其他AGV在LM10，且其他AGV目标不是LM12 且其他AGV任务优先级高，则让其他AGV先申请LM11资源
                while ("LM10".equals(otherPos)
                        && !"LM12".equals(otherTargetPos)
                        && WorkStatusEnum.WORK.getCode().equals(otherAgvDto.getWorkStatus())
                        && Long.parseLong(currentAgvDto.getTaskNo()) > Long.parseLong(otherAgvDto.getTaskNo())) {
                    otherPos = this.getOtherAgvPos(clientId);
                    currentAgvDto = this.getCurrentAgvDto(clientId);
                    otherAgvDto = this.getOtherAgvDto(clientId);
                    otherTargetPos = this.getOtherTargetPos(otherAgvDto.getTargetPos());
                    TimeUnit.SECONDS.sleep(1);
                }

                //当其他AGV在LM9，则让其他AGV先申请LM11资源 否则有可能堵住
                while ("LM9".contains(otherPos)
                        && WorkStatusEnum.WORK.getCode().equals(otherAgvDto.getWorkStatus())) {
                    otherPos = this.getOtherAgvPos(clientId);
                    otherAgvDto = this.getOtherAgvDto(clientId);
                    TimeUnit.SECONDS.sleep(1);
                }
            } else if (currentPos.equals("LM9")) {
                //LM9点位允许优先申请LM11资源，否则可能被堵住
            }
        }

        if (resourceId.intValue() == 142) {//LM5,LM38
            String currentPos = this.getCurrentAgvPos(clientId);
            String otherPos = this.getOtherAgvPos(clientId);
            String currentTargetPos = currentAgvDto.getTargetPos().getCode();
            String otherTargetPos = this.getOtherTargetPos(otherAgvDto.getTargetPos());
            if (currentPos.equals("LM45")) {
                List<String> posList = List.of("LM40", "LM4");
                //当其他AGV在LM40,LM4 则让其他AGV先申请"LM5,LM38"资源 否则会堵住
                while (posList.contains(otherPos) && WorkStatusEnum.WORK.getCode().equals(otherAgvDto.getWorkStatus())) {
                    otherPos = this.getOtherAgvPos(clientId);
                    otherAgvDto = this.getOtherAgvDto(clientId);
                    TimeUnit.SECONDS.sleep(1);
                }
                //当其他AGV在LM51 则让其他AGV先申请"LM5,LM38"资源 否则会堵住
                while ("LM51".equals(otherPos) && WorkStatusEnum.WORK.getCode().equals(otherAgvDto.getWorkStatus())) {
                    otherPos = this.getOtherAgvPos(clientId);
                    otherAgvDto = this.getOtherAgvDto(clientId);
                    TimeUnit.SECONDS.sleep(1);
                }
            }
        }

        if (resourceId.intValue() == 143) {//LM41,LM3,LM35
            String currentPos = this.getCurrentAgvPos(clientId);
            String otherPos = this.getOtherAgvPos(clientId);
            String currentTargetPos = currentAgvDto.getTargetPos().getCode();
            String otherTargetPos = this.getOtherTargetPos(otherAgvDto.getTargetPos());
            if (currentPos.equals("LM46")) {
                List<String> posList = List.of("LM39", "LM2");
                //当其他AGV在LM39,LM2 则让其他AGV先申请"LM41,LM3,LM35"资源 否则会堵住
                while (posList.contains(otherPos) && WorkStatusEnum.WORK.getCode().equals(otherAgvDto.getWorkStatus())) {
                    otherPos = this.getOtherAgvPos(clientId);
                    otherAgvDto = this.getOtherAgvDto(clientId);
                    TimeUnit.SECONDS.sleep(1);
                }
                //当其他AGV在LM51 则让其他AGV先申请LM41,LM3,LM35资源
                while ("LM51".equals(otherPos) && WorkStatusEnum.WORK.getCode().equals(otherAgvDto.getWorkStatus())) {
                    otherPos = this.getOtherAgvPos(clientId);
                    otherAgvDto = this.getOtherAgvDto(clientId);
                    TimeUnit.SECONDS.sleep(1);
                }
            } else if (currentPos.equals("LM39")) {
                //当其他AGV在LM51 且目标点位是 LM12,  则让其他AGV先申请LM41,LM3,LM35资源
                while ("LM51".equals(otherPos)
                        && "LM12".equals(otherTargetPos)
                        && WorkStatusEnum.WORK.getCode().equals(otherAgvDto.getWorkStatus())) {
                    otherPos = this.getOtherAgvPos(clientId);
                    otherAgvDto = this.getOtherAgvDto(clientId);
                    TimeUnit.SECONDS.sleep(1);
                }
            } else if (currentPos.equals("LM51")) {
                while (List.of("LM46", "LM39").contains(otherPos)
                        && WorkStatusEnum.WORK.getCode().equals(otherAgvDto.getWorkStatus())
                        && Long.parseLong(currentAgvDto.getTaskNo()) > Long.parseLong(otherAgvDto.getTaskNo())) {
                    otherPos = this.getOtherAgvPos(clientId);
                    currentAgvDto = this.getCurrentAgvDto(clientId);
                    otherAgvDto = this.getOtherAgvDto(clientId);
                    TimeUnit.SECONDS.sleep(1);
                }
            }
        }
    }

}